package com.anji.robotnav;

import java.awt.*;
import java.awt.geom.*;
import java.awt.image.BufferedImage;
import java.awt.image.Raster;
import java.awt.RenderingHints;
import java.io.File;
import java.io.IOException;
import java.text.DecimalFormat;

import javax.imageio.ImageIO;
import javax.media.j3d.Appearance;
import javax.media.j3d.VirtualUniverse;
import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import ojc.util.TimeProfiler;

import org.apache.log4j.Logger;
import org.jgap.*;

import simbad.sim.Agent;
import simbad.sim.Arch;
import simbad.sim.Box;
import simbad.sim.CameraSensor;
import simbad.sim.DifferentialKinematic;
import simbad.sim.EnvironmentDescription;
import simbad.sim.RobotFactory;
import simbad.sim.SensorMatrix;
import simbad.sim.SimpleAgent;
import simbad.sim.Simulator;
import simbad.sim.Wall;
import simbad.sim.World;

import com.anji.hyperneat.*;
import com.sun.j3d.utils.geometry.Cylinder;
import com.sun.j3d.utils.geometry.Primitive;
import com.sun.j3d.utils.geometry.Sphere;
//import com.sun.opengl.impl.GLContextImpl;

public class RobotNavFitnessFunction extends HyperNEATFitnessFunction {
	public static final String SUB_TASK_KEY = "rn.subtask";
	public static final String FRAME_RATE_KEY = "rn.framerate";
	public static final String FIELD_WIDTH_KEY = "rn.field.width";
	public static final String FIELD_HEIGHT_KEY = "rn.field.height";
	public static final String OBJECT_SPEED_KEY = "rn.object.speed";
	public static final String OBJECT_SIZE_KEY = "rn.object.size";
	public static final String ROBOT_SIZE_KEY = "rn.robot.size";
	public static final String TARGET_SHAPE_KEY = "rn.object.target.shape";
	public static final String NONTARGET_SHAPE_KEY = "rn.object.nontarget.shape";
	public static final String ROBOT_SPEED_KEY = "rn.robot.speed";
	public static final String ROBOT_FOV_KEY = "rn.robot.fieldofview";
	public static final String SAVE_IMAGES_KEY = "rn.saveimages";
	public static final String FITNESS_WEIGHT_PC_KEY = "rn.fitness.weight.percentcorrect";
	public static final String FITNESS_WEIGHT_DIST_AVG_RMS_KEY = "rn.fitness.weight.distanceavg.rms";
	public static final String FITNESS_WEIGHT_DIST_FINAL_ACTUAL_KEY = "rn.fitness.weight.distancefinal.actual";
	public static final String FITNESS_WEIGHT_DIST_FINAL_RMS_KEY = "rn.fitness.weight.distancefinal.rms";
	public static final String FITNESS_WEIGHT_TIME_KEY = "rn.fitness.weight.timerequired";
	public static final String PERFORMANCE_METRIC_KEY = "rn.performance.metric";
	public static final String MAX_TRIAL_TIME_KEY = "rn.maxtrialtime";
	public static final String MAX_TEST_TIME_KEY = "rn.maxtesttime";
	public static final String NUM_TRIALS_KEY = "rn.numtrials";
	public static final String ANTIALIAS_KEY = "rn.antialias";
	
	
	DecimalFormat nf2 = new DecimalFormat("0.00");
	DecimalFormat nf4 = new DecimalFormat("0.0000");
	
	private static int maxFitnessValue = 1000000;
	
	private float bestPerformanceSoFar = 0;
	private float bestFitnessSoFar = 0;
	public float bestPCSoFar = 0;
    
	private int numTrials = 4;
	private int maxNumTrials = numTrials;
	private boolean increaseNumTrials;
	private int numTests = numTrials;
	private float maxTrialTime = 5;
	private float maxTestTime = 10;
	private int frameRate = 5; //fps
    private int maxSimSteps;
	private float fitnessWeightPC = 0;
    private float fitnessWeightDistAvgRMS = 0;
    private float fitnessWeightDistFinalActual = 0;
    private float fitnessWeightDistFinalRMS = 1;
    private float fitnessWeightTime = 0;
    private String perfMetric = FITNESS_WEIGHT_PC_KEY;
    
    private int subtask = 1;
    private float fieldWidth = 5; // m
    private float fieldHeight = 5;
    private float objectSize = 0.5f;
    private float objectSpeed = 0; // m/s
    private float robotSize = 0.5f;
    private float robotSpeed = 1f; // m/s
    private float robotFOV = (float)Math.toRadians(90);
    private int numObjects = 1;
    private String targetShape = "sphere";
    private String nonTargetShape = "cube";
    
    private float stalledTimeOut = 0.5f; //in seconds
    
    private float maxDist;
    
    private boolean saveImages = false;
    private boolean printedFirst = false;
    private String imageDir;
    int[] imageScaleActivation = {2, 16, 16}; //size (in pixels) of a square representing the activation of a neuron (per layer)
	int[] imageScaleWeights = {3, 9}; //size (in pixels) of a square representing the value of a weight
	int imageNegDotSize = 1; //size of black square/dot in middle of weight value square to indicate negative value
	int imageSpacing = 4;
    
    float connectionWeightMin;
	float connectionWeightMax;
	float weightRange;	
	
	
	Simulator[] simulator;
	World[] world;
	Robot[] robot;
	RNObject[][] objects;
	
	Vector3d[] trialPositionsRobot;
	double[] trialRotationsRobot;
	Vector3d[][] trialPositionsObjects;
	double[][] trialRotationsObjects;
	
	Vector3d[] testPositionsRobot;
	double[] testRotationsRobot;
	Vector3d[][] testPositionsObjects;
	double[][] testRotationsObjects;
	
	Color3f targetColour, nonTargetColour; //nontarget not used in subtask 1
	
	boolean antialias = false;
    
    public void init(com.anji.util.Properties props) {
    	super.init(props);
    	
    	subtask = props.getIntProperty(SUB_TASK_KEY, subtask);
        frameRate = props.getIntProperty(FRAME_RATE_KEY, frameRate);
        fieldWidth = props.getFloatProperty(FIELD_WIDTH_KEY, fieldWidth);
        fieldHeight = props.getFloatProperty(FIELD_HEIGHT_KEY, fieldHeight);
        objectSize = props.getFloatProperty(OBJECT_SIZE_KEY, objectSize);
        objectSpeed = props.getFloatProperty(OBJECT_SPEED_KEY, objectSpeed);
        targetShape = props.getProperty(TARGET_SHAPE_KEY, targetShape);
        nonTargetShape = props.getProperty(NONTARGET_SHAPE_KEY, nonTargetShape);
        robotSize = props.getFloatProperty(ROBOT_SIZE_KEY, robotSize);
        robotSpeed = props.getFloatProperty(ROBOT_SPEED_KEY, robotSpeed);
        robotFOV = (float)Math.toRadians(props.getFloatProperty(ROBOT_FOV_KEY, robotFOV));
        antialias = props.getBooleanProperty(ANTIALIAS_KEY, antialias);
        
        saveImages = props.getBooleanProperty(SAVE_IMAGES_KEY, saveImages);
        fitnessWeightPC = props.getFloatProperty(FITNESS_WEIGHT_PC_KEY, fitnessWeightPC);
        fitnessWeightDistAvgRMS = props.getFloatProperty(FITNESS_WEIGHT_DIST_AVG_RMS_KEY, fitnessWeightDistAvgRMS);
        fitnessWeightDistFinalActual = props.getFloatProperty(FITNESS_WEIGHT_DIST_FINAL_ACTUAL_KEY, fitnessWeightDistFinalActual);
        fitnessWeightDistFinalRMS = props.getFloatProperty(FITNESS_WEIGHT_DIST_FINAL_RMS_KEY, fitnessWeightDistFinalRMS);
        fitnessWeightTime = props.getFloatProperty(FITNESS_WEIGHT_TIME_KEY, fitnessWeightTime);
        perfMetric = props.getProperty(PERFORMANCE_METRIC_KEY, perfMetric);
        maxTrialTime = props.getFloatProperty(MAX_TRIAL_TIME_KEY, maxTrialTime);
        maxTestTime = props.getFloatProperty(MAX_TEST_TIME_KEY, maxTestTime);
        numTrials = props.getIntProperty(NUM_TRIALS_KEY, numTrials);
        maxNumTrials = numTrials; //might be using incremental training (in which case we adjust numTrials a bit later)
        
        maxSimSteps = Math.round(maxTrialTime * frameRate);
        
        maxDist = (float)Math.sqrt(fieldWidth*fieldWidth + fieldHeight*fieldHeight);
        
	    connectionWeightMin = props.getFloatProperty(HyperNEATTranscriber.HYPERNEAT_CONNECTION_WEIGHT_MIN);
		connectionWeightMax = props.getFloatProperty(HyperNEATTranscriber.HYPERNEAT_CONNECTION_WEIGHT_MAX);
		
		targetColour = new Color3f(1,1,1);
		nonTargetColour = targetColour;
		
		switch (subtask) {
		case 1:
			//numTrials = 2;
			numObjects = 1;
			numTests = 180/5; //180 deg, 5 deg rotation per test
			break;
		case 2:
			numObjects = 1;
			numTests = 6;
			break;
		case 3:
			numObjects = 1;
			numTests = 0; //numTrials * 2;
			break;
		}
		
		int numThreads = getNumThreads();
		simulator = new Simulator[numThreads];
		world = new World[numThreads];
		robot = new Robot[numThreads];
		objects = new RNObject[numThreads][numObjects];
		
		trialPositionsRobot = new Vector3d[maxNumTrials];
		trialRotationsRobot = new double[maxNumTrials];
		trialPositionsObjects = new Vector3d[maxNumTrials][numObjects];
		trialRotationsObjects = new double[maxNumTrials][numObjects];
		
		testPositionsRobot = new Vector3d[numTests];
		testRotationsRobot = new double[numTests];
		testPositionsObjects = new Vector3d[numTests][numObjects];
		testRotationsObjects = new double[numTests][numObjects];
		
		
		//unique directory for images for this run
		imageDir = props.getProperty("output.dir");
		
		weightRange = connectionWeightMax - connectionWeightMin;
		
		bestPerformanceSoFar = targetPerformanceType == 1 ? 0 : Float.MAX_VALUE;;
    }
    
    
    /**
     * @return maximum possible fitness value for this function
     */
    public int getMaxFitnessValue() {
        return maxFitnessValue;
    }
    
    
    /**
     * Initialise data for the current evaluation run (for each generation).
     */
    public void initialiseEvaluation() {
    	if (increaseNumTrials && numTrials < maxNumTrials) {
    		increaseNumTrials = false;
    		numTrials += 2;
    		if (numTrials > maxNumTrials)
    			numTrials = maxNumTrials;
    		bestFitnessSoFar *= 0.95; //so we can see the progress via saved images
    		logger.info("Increasing number of trials to " + numTrials);
    	}
    	
    	//generate robot and object(s) (including targets) initial positions for each trial
    	double nominalDistance = maxTrialTime * robotSpeed + ((robotSize + objectSize) / 2);
    	double distance;
    	for (int trial = 0; trial < numTrials; trial++) {
	    	//trialRotationsRobot[trial] = (trial * Math.PI) / (numTrials-1);
    		trialRotationsObjects[trial][0] = 0;
	    	
	    	switch (subtask) {
			case 1:
				//double[] trialAngles = 	   {-180, -90, -10,  10,   -20,  20,   -30,  30,   -40,  40};
		    	//double[] trialDistAdjust = {0.6, 0.3, 0.03, 0.03, 0.06, 0.06, 0.09, 0.09, 0.12, 0.12}; //allow time for turning around, can turn at about 25 deg per time step
		    	//double[] trialAngles = 	   {-10,  10,   -20,  20,   -30,  30,   -40,  40, -90, -180};
		    	//double[] trialDistAdjust = {0.03, 0.03, 0.06, 0.12, 0.12, 0.09, 0.12, 0.12, 0.3, 0.6}; //allow time for turning around, can turn at about 25 deg per time step
				
				double[] trialAngles = 	   {-14, 14, -28,  28, -42,  42, -90, -180};
		    	
		    	//double distance = nominalDistance - (trialDistAdjust[trial] + 0.06); //add some margin to allow imperfect solutions
		    	
		    	//allow time for turning around, require rate of about 25 deg per time step, add (subtract) some margin to allow imperfect solutions
		    	distance = (nominalDistance - (Math.abs(trialAngles[trial]) / 250)) * 0.95;
		    	
				trialRotationsRobot[trial] = Math.toRadians(trialAngles[trial]);
		    	trialPositionsRobot[trial] = new Vector3d(-distance/2, robotSize * 0.125, 0);
				
		    	trialPositionsObjects[trial][0] = new Vector3d(distance/2, objectSize/2, 0); 
				break;
			case 2:
				Vector3d pos1 = new Vector3d(-objectSize*4, robotSize * 0.125, 0);
				Vector3d pos2 = new Vector3d(objectSize*1.5, objectSize/2, 0);
				switch (trial) {
				case 0:
					trialRotationsRobot[trial] = 0;
					trialPositionsRobot[trial] = pos1;
			    	
					trialRotationsObjects[trial][0] = 0;
					trialPositionsObjects[trial][0] = pos2;
					break;
				case 1:
					trialRotationsRobot[trial] = Math.PI;
					trialPositionsRobot[trial] = pos2;
			    	
					trialRotationsObjects[trial][0] = 0;
					trialPositionsObjects[trial][0] = pos1;
					break;
				}	
				break;
			case 3:
				double delta = objectSize * 3; //distance between obstacles
				double maxWidth = delta * maxNumTrials + delta * 3; //max width of set of obstacles for any trial
				double totalWidth = maxWidth * maxNumTrials;
				double xOffset = -totalWidth/2;
				double trialObsCentre = xOffset + (trial + 0.5) * maxWidth; //centre of set of obstacles for this trial
				distance = nominalDistance * 0.9;
				
				trialRotationsRobot[trial] = -Math.PI/2;
		    	trialPositionsRobot[trial] = new Vector3d(trialObsCentre, robotSize * 0.125, -distance/2);
		    	
				trialRotationsObjects[trial][0] = 0;
				trialPositionsObjects[trial][0] = new Vector3d(trialObsCentre, objectSize/2, distance/2);
				break;
	    	}
    	}
    	
    	//generate robot and object(s) (including targets) initial positions for each test
    	for (int test = 0; test < numTests; test++) {
	    	testRotationsObjects[test][0] = 0;
			
	    	switch (subtask) {
			case 1:
				testRotationsRobot[test] = (test * Math.PI) / numTests;
				
				distance = nominalDistance * (((double) test / numTests) + 1);
				testPositionsRobot[test] = new Vector3d(-distance/2, robotSize/2, 0);
				testPositionsObjects[test][0] = new Vector3d(distance/2, objectSize/2, 0);
		    	break;
		    	
			case 2:
				testPositionsRobot[test] = new Vector3d(-objectSize*4, robotSize * 0.125, 0);
				testRotationsRobot[test] = 0;
		    					
				double x = objectSize * 3.5 * ((test % 2) - 1) + 1;
		    	double z = objectSize * 5 * ((test / 2) - 1);
		    	testPositionsObjects[test][0] = new Vector3d(x, objectSize/2, z);
		    	break;
	    	}
    	}
    }
    
    
    
    private void createSim(int threadIndex, GridNet network) {
    	Agent[] agents = new Agent[1 + numObjects];
    	int agentIndex = 0;
    	robot[threadIndex] = new Robot(new Vector3d(), threadIndex, robotSpeed, robotSize, robotSize*0.5f, network);
    	agents[agentIndex++] = robot[threadIndex]; 
    	for (int obj = 0; obj < numObjects; obj++) {
    		float width, height;
    		boolean isTarget = obj == 0;
			Color3f colour = targetColour;
			String shape = targetShape;
    		if (!isTarget) {
    			colour = nonTargetColour;
    			shape = nonTargetShape;
			}
			
    		width = objectSize;
			height = objectSize;
			
			objects[threadIndex][obj] = new RNObject(new Vector3d(), "Object-" + threadIndex + "-" + obj, objectSpeed, width, height, colour, shape);
			
			agents[agentIndex++] = objects[threadIndex][obj];
    	}
    	Environment env = new Environment(fieldWidth, fieldHeight, agents);
    	
    	float wallThickness = 0.2f;
		float wallHeight = objectSize+0.1f;
		//float wallHeight = 0.01f;
		Color3f boundaryWallColour = new Color3f(0,0,0);
		
    	//boundary walls
        Wall wl = new Wall(new Vector3d(-fieldWidth/2+wallThickness/2, 0, 0), fieldHeight, wallThickness, wallHeight, boundaryWallColour, env);
        wl.rotate90(1);
        env.add(wl);
        
        Wall wr = new Wall(new Vector3d(fieldWidth/2-wallThickness/2, 0, 0), fieldHeight, wallThickness, wallHeight, boundaryWallColour, env);
        wr.rotate90(1);
        env.add(wr);
        
        Wall wt = new Wall(new Vector3d(0, 0, -fieldHeight/2+wallThickness/2), fieldWidth, wallThickness, wallHeight, boundaryWallColour, env);
        env.add(wt);
        
        Wall wb = new Wall(new Vector3d(0, 0, fieldHeight/2-wallThickness/2), fieldWidth, wallThickness, wallHeight, boundaryWallColour, env);
        env.add(wb);
    	
    	switch (subtask) {
		case 1:
			//nothing to do
			break;
		case 2:
			//maze walls
			
			Wall w1 = new Wall(new Vector3d(-objectSize*2 - wallThickness/2, 0, 0), objectSize*4 + wallThickness, wallThickness, wallHeight, env);
	        w1.rotate90(1);
	        env.add(w1);
	        
	        Wall w3 = new Wall(new Vector3d(-wallThickness/2, 0, objectSize*2 + wallThickness/2), objectSize*4 + wallThickness, wallThickness, wallHeight, env);
	        env.add(w3);
	        
	        Wall w5 = new Wall(new Vector3d(-wallThickness/2, 0, -objectSize*2 - wallThickness/2), objectSize*4 + wallThickness, wallThickness, wallHeight, env);
	        env.add(w5);
	        
			break;
		case 3:
			env.wallColor = env.white;
	    	
			double delta = objectSize * 3; //distance between obstacles
			double maxWidth = delta * maxNumTrials + delta * 3; //max width of set of obstacles for any trial
			double totalWidth = maxWidth * maxNumTrials;
			double xOffset = -totalWidth/2;
			
			for (int trial = 0; trial < maxNumTrials; trial++) {
				double trialObsCentre = xOffset + (trial + 0.5) * maxWidth; //centre of set of obstacles for this trial
				double obsWidth = trial * delta; //actual width of set of obstacles
			
				for (int obs = 0; obs <= trial; obs++) {
					double obsCentre = obs * delta - obsWidth / 2; //relative position of obstacle
					
					Wall o = new Wall(new Vector3d(trialObsCentre + obsCentre, 0, 0), objectSize, objectSize, objectSize, env);
					env.add(o);
				}
			}
		}
    	
    	world[threadIndex] = new World(env, new Dimension(Math.round(fieldWidth * 100), Math.round(fieldHeight*100)));
		simulator[threadIndex] = new Simulator(world[threadIndex], env);
		simulator[threadIndex].setFramesPerSecond(frameRate);
    }
    
    
    
    protected int evaluate(Chromosome genotype, com.anji.hyperneat.GridNet substrate, int threadIndex) {
    	if (simulator[threadIndex] == null) {
    		System.out.println("Creating new simulator");
    		createSim(threadIndex, substrate);
    	}
    	Simulator sim = simulator[threadIndex];
    	
    	robot[threadIndex].setNetwork(substrate);
    	
        float avgDistRMS = 0;
        float avgDistFinalActual = 0;
        float avgDistFinalRMS = 0;
        float percentCorrect = 0;
        float avgTime = 0;
        
        Point3d robotCentre = new Point3d(), previousRobotCentre;
        Point3d targetCentre = new Point3d();
        
        int rmpFactor=2; //root mean power (RMS)
    	
        for (int trial = 0; trial < numTrials; trial++) {
        	sim.resetAgents();
        	sim.initBehaviors();
    		
        	//set robot and target(s) to initial positions for this trial
        	robot[threadIndex].resetPositionRotationAt(trialPositionsRobot[trial], trialRotationsRobot[trial]);
        	
        	for (int obj = 0; obj < numObjects; obj++) {
    		    objects[threadIndex][obj].resetPositionRotationAt(trialPositionsObjects[trial][obj], trialRotationsObjects[trial][obj]);
        	}
        	
        	double dist = maxDist, previousDist = maxDist, distSum = 0;
        	int simStepCount;
        	previousRobotCentre = null;
        	int stalledCount = 0;
        	for (simStepCount = 0; simStepCount < maxSimSteps; simStepCount++) {
        		previousDist = dist;
        		
        		sim.simulateOneStep();
            	
            	//get distance
            	robot[threadIndex].getCoords(robotCentre);
            	objects[threadIndex][0].getCoords(targetCentre);
            	dist = robotCentre.distance(targetCentre) - (robotSize + objectSize) / 2;
                if (objects[threadIndex][0].interactionDetected()) {
                //if (dist <= (robotSize + objectSize) / 2 + robotSpeed / frameRate) { //if within the distance the robot can move in one time step
                	percentCorrect++;
                	dist = 0; //compensate for case of robot and object bouncing off each other
                	//distSum += dist;
                	//System.out.println("collided");
                	break;
                }
                distSum += dist;
                if (previousRobotCentre != null && robotCentre.distance(previousRobotCentre) < robotSpeed*0.01 && 
                		Math.abs(robot[threadIndex].getRotationalVelocity()) < Math.toRadians(5))
                	stalledCount++;
                else
                	stalledCount = 0;
                
                //if we've stalled (eg become stuck on something) then end simulation
                if (stalledCount > stalledTimeOut * frameRate) {
                	//System.out.println(genotype.getId() + " stalled on trial " + t + " at step " + simStepCount);
                	distSum += dist * (maxSimSteps - simStepCount - 1); //remaining time is spent at current distance
                	simStepCount = maxSimSteps;
                	break;
                }
            }
        	
        	avgTime += (float)simStepCount / frameRate;
            avgDistRMS += (float)Math.pow((distSum / maxSimSteps) / maxDist, rmpFactor);
            //avgDistFinal += (float)Math.pow(dist / maxDist, 3);
            avgDistFinalActual += dist;
            avgDistFinalRMS += (float)Math.pow(previousDist / maxDist, rmpFactor);
            //avgDistFinal += dist / maxDist;
        }
        
        if (Math.round(percentCorrect) == numTrials) {
        	increaseNumTrials = true;
        }
        
        avgDistRMS = Math.min(1, avgDistRMS / numTrials);
        avgDistFinalRMS = Math.min(1, avgDistFinalRMS / numTrials);
        percentCorrect /= maxNumTrials;
        avgTime /= numTrials;
        
        //System.out.println(avgDist + ", " + avgDistFinal + ", " + percentCorrect + ", " + avgTime);  
               
        avgDistFinalActual /= numTrials;
        
        //calculate fitness according to fitness function type weightings
        //float fitness = fitnessWeightPC * percentCorrect + fitnessWeightDist * (1 / (1+avgDist));
        float fitness = fitnessWeightPC * percentCorrect + 
        				fitnessWeightDistAvgRMS * (1 - (float) Math.pow(avgDistRMS, 1.0/rmpFactor)) + 
        				fitnessWeightDistFinalRMS * (1 - (float) Math.pow(avgDistFinalRMS, 1.0/rmpFactor)) + 
        				fitnessWeightTime * (1 - avgTime / ((float) maxSimSteps / frameRate));
        
        fitness /= fitnessWeightPC + fitnessWeightDistAvgRMS + fitnessWeightDistFinalRMS + fitnessWeightTime;
        
        
        float performance;
      	if (perfMetric.equals(FITNESS_WEIGHT_PC_KEY))
      		performance = percentCorrect;
        else if (perfMetric.equals(FITNESS_WEIGHT_DIST_AVG_RMS_KEY))
        	performance = avgDistRMS;
        else if (perfMetric.equals(FITNESS_WEIGHT_DIST_FINAL_ACTUAL_KEY))
        	performance = avgDistFinalActual;
        else if (perfMetric.equals(FITNESS_WEIGHT_DIST_FINAL_RMS_KEY))
        	performance = avgDistFinalRMS;
        else if (perfMetric.equals(FITNESS_WEIGHT_TIME_KEY))
        	performance = avgTime;
        else 
        	performance = fitness;
      	
      	genotype.setPerformanceValue(performance);
      	
      	float nextNoteworthyFitnessFactor = 0.01f;
      	float nextNoteworthyFitness = bestFitnessSoFar + (1-bestFitnessSoFar) * nextNoteworthyFitnessFactor;
      	boolean saveImagesNow = saveImages && 
      		(
      			(
      				((targetPerformanceType == 1 && performance >= bestPerformanceSoFar + 0.01f) || (targetPerformanceType == 0 && performance <= bestPerformanceSoFar - 0.01f))
      				|| fitness >= nextNoteworthyFitness
      			) || 
      			(
      				lastBestChrom == genotype && 
      					((targetPerformanceType == 1 && lastBestPerformance >= scalePerformance) || (targetPerformanceType == 0 && lastBestPerformance <= scalePerformance)) 
      			) || 
      			!printedFirst
      		);
	    
      	if ((targetPerformanceType == 1 && performance >= bestPerformanceSoFar + 0.01f) || (targetPerformanceType == 0 && performance <= bestPerformanceSoFar - 0.01f))
			bestPerformanceSoFar = performance;
      	if (fitness >= nextNoteworthyFitness) {
      		bestFitnessSoFar = fitness;
      		System.out.println("next noteworthy fitness: " + (bestFitnessSoFar + (1-bestFitnessSoFar) * nextNoteworthyFitnessFactor));
      	}
      	if (percentCorrect > bestPCSoFar)
      		bestPCSoFar = percentCorrect;
      	
    	if (saveImagesNow) {
        	printedFirst = true;
      		System.out.println("saving images for " + genotype.getId() + ", performance: " + nf2.format(performance) + ", fitness: " + nf4.format(fitness));
    		generateImages(genotype, substrate, threadIndex, fitness, performance);
      	}
  		
        return (int) Math.round(fitness * maxFitnessValue);
    }
    
    
    
    
    protected void scale(int scaleCount, int scaleFactor) {
    	/*
    	//get ratio of shape size to image size (this should be maintained during scale).
    	float ratioW[] = new float[depth];
    	float ratioH[] = new float[depth];
    	for (int l = 0; l < width.length-1; l++) {
            ratioW[l] = (float) width[l] / shapeSize;
            ratioH[l] = (float) height[l] / shapeSize;
    	}
    	
    	//System.out.println(ratioW + ", " + ratioH);
    	
    	//adjust shape size
    	if (scaleFactor % 2 == 0 && shapeSize % 2 == 1) //if scaleFactor is even but shapeSize is odd
    		shapeSize = (shapeSize/2) * scaleFactor * 2 + 1; //preserve oddness of conn range
    	else
    		shapeSize *= scaleFactor;
    	
    	String layerSizeString = "";
    	for (int l = 0; l < depth; l++) {
    		width[l] = Math.max(1, Math.round(shapeSize * ratioW[l]));
    		height[l] = Math.max(1, Math.round(shapeSize * ratioH[l]));
    		
    		layerSizeString += width[l] + "x" + height[l] + ", ";
    	}
    	//connectionRange = shapeSize/2;
		connectionRange = shapeSize;
    	
    	AffineTransform scaleTransform = AffineTransform.getScaleInstance(scaleFactor, scaleFactor);
		for (int s = 0; s < numShapesInLib; s++)
        	shapes[s].transform(scaleTransform);
		
			
		logger.info("Scale performed: layer sizes: " + layerSizeString + "shape size: " + shapeSize + ", conn range: " + connectionRange);
		*/
    }
    
    
    
    private void generateImages(Chromosome genotype, com.anji.hyperneat.GridNet substrate, int threadIndex, float f, float p) {
    	//generate image for weight patterns for each neuron in each layer45
    	String outputDir = imageDir + generation + "-" + scaleCount + "-" + genotype.getId() + "-f" + nf4.format(f) + "-p" + nf4.format(p);
    	    	
    	//Generate image for weights
		float weightRange = connectionWeightMax - connectionWeightMin;
		int connectionRange = getConnectionRange();
		BufferedImage[] weightImage = new BufferedImage[depth-1];
		int xOffset = 0, yOffset = 0;
		int imageWeightLayerMaxWidth = 0;
		int imageWeightLayerTotalHeight = 0;
		for (int tz = 1; tz < depth; tz++) { //tz-1 is source layer
			int imageWidth = width[tz] * (width[tz-1] * imageScaleWeights[tz-1] + imageSpacing/2) - imageSpacing/2;
			int imageHeight = height[tz] * (height[tz-1] * imageScaleWeights[tz-1] + imageSpacing/2) - imageSpacing/2;
			
			imageWeightLayerMaxWidth = Math.max(imageWeightLayerMaxWidth, imageWidth);
			imageWeightLayerTotalHeight += imageHeight + imageSpacing;
			
			weightImage[tz-1] = new BufferedImage(imageWidth, imageHeight, BufferedImage.TYPE_INT_RGB);
			Graphics2D g = weightImage[tz-1].createGraphics();
			
			g.setColor(new Color(0, 0, 127));
			g.fillRect(0, 0, imageWidth, imageHeight);
			
			for (int ty = 0; ty < height[tz]; ty++) {
				for (int tx = 0; tx < width[tz]; tx++) {
					xOffset = tx * (width[tz-1] * imageScaleWeights[tz-1] + imageSpacing/2);
					yOffset = ty * (height[tz-1] * imageScaleWeights[tz-1] + imageSpacing/2);
					
					//initialise to 0 weight value color for case where layers are not fully connected
		            //g.setColor(new Color(127, 127, 127));
		            //g.fill(new Rectangle(xOffset, yOffset, width[tz-1] * imageScaleWeights[tz-1], height[tz-1] * imageScaleWeights[tz-1]));
		            
		        	//calculate dimensions of this weight target matrix (bounded by grid edges)
					int dy = Math.min(height[tz-1] - 1, ty + connectionRange) - Math.max(0, ty - connectionRange) + 1;
					int dx = Math.min(width[tz-1] - 1, tx + connectionRange) - Math.max(0, tx - connectionRange) + 1;
					float[][] w = substrate.getWeights()[tz-1][ty][tx][0];
					
					for (int wy = 0, sy = Math.max(0, ty - connectionRange); wy < dy; wy++, sy++) {
						for (int wx = 0, sx = Math.max(0, tx - connectionRange); wx < dx; wx++, sx++) {
							int color = (int) (((w[wy][wx] - connectionWeightMin) / weightRange) * 255);
							g.setColor(new Color(color, color, color));
							g.fillRect(xOffset + sx * imageScaleWeights[tz-1], yOffset + sy * imageScaleWeights[tz-1], imageScaleWeights[tz-1], imageScaleWeights[tz-1]);
							//if weight value is negative indicate with a black dot
							if (w[wy][wx] < 0) {
								g.setColor(Color.black);
								g.fillRect(xOffset + sx * imageScaleWeights[tz-1] + imageScaleWeights[tz-1]/2 - imageNegDotSize/2, yOffset + sy * imageScaleWeights[tz-1] + imageScaleWeights[tz-1]/2 - imageNegDotSize/2, imageNegDotSize, imageNegDotSize);
							}
						}
		        	}
				}
			}
		}
		imageWeightLayerTotalHeight -= imageSpacing;
		
		int imageWidth = imageWeightLayerMaxWidth + imageSpacing*2; //add border
		int imageHeight = imageWeightLayerTotalHeight + imageSpacing*2;
		
		BufferedImage output = new BufferedImage(imageWidth, imageHeight, BufferedImage.TYPE_INT_RGB);
		Graphics2D g = output.createGraphics();
		g.setColor(new Color(0, 0, 127));
		g.fillRect(0, 0, imageWidth, imageHeight);
		
		yOffset = imageSpacing;
		for (int layer = 0; layer < depth-1; layer++) {
			g.drawImage(weightImage[layer], imageSpacing, yOffset, null);
			yOffset += weightImage[layer].getHeight() + imageSpacing;
		}
		
		writeImage(output, outputDir, "weights");
    		
    	
    	
		//generate image for each sim step of each trial
		Simulator sim = simulator[threadIndex];
    	Point3d robotCentre = new Point3d(), previousRobotCentre;
        Point3d targetCentre = new Point3d();
    	
        for (int trial = 0; trial < numTrials; trial++) {
        	sim.resetAgents();
        	sim.initBehaviors();
    		
        	//set robot and target(s) to initial positions for this trial
        	robot[threadIndex].resetPositionRotationAt(trialPositionsRobot[trial], trialRotationsRobot[trial]);
        	
        	for (int obj = 0; obj < numObjects; obj++) {
    		    objects[threadIndex][obj].resetPositionRotationAt(trialPositionsObjects[trial][obj], trialRotationsObjects[trial][obj]);
        	}
        	
        	int simStepCount;
        	previousRobotCentre = null;
        	int stalledCount = 0;
        	boolean collided = false;
        	for (simStepCount = 0; simStepCount < maxSimSteps; simStepCount++) {
        		output = generateImagesForSimStep(genotype, substrate, threadIndex, true); //calls sim.simulateOneStep
        		
        		//get distance
            	robot[threadIndex].getCoords(robotCentre);
            	objects[threadIndex][0].getCoords(targetCentre);
            	//dist = robotCentre.distance(targetCentre) - (robotSize + objectSize) / 2;
            	//if (dist <= robotSpeed / frameRate) { //if within the distance the robot can move in one time step
                if (objects[threadIndex][0].interactionDetected()) {
                	collided = true;
                }
                
                writeImage(output, outputDir + File.separatorChar + "trial", trial + "-" + simStepCount);
            	
            	if (collided) {
            		simStepCount++;
            		break;
            	}
                
                if (previousRobotCentre != null && robotCentre.distance(previousRobotCentre) < robotSpeed*0.01 && 
                		Math.abs(robot[threadIndex].getRotationalVelocity()) < Math.toRadians(5))
                    
                	stalledCount++;
                else
                	stalledCount = 0;
                
                //if we've stalled (eg become stuck on something) then end simulation
                if (stalledCount > stalledTimeOut * frameRate) {
                	simStepCount = maxSimSteps;
                	break;
                }
                previousRobotCentre = (Point3d) robotCentre.clone();
                
            }
        	output = generateImagesForSimStep(genotype, substrate, threadIndex, false);
        	if (collided) {
	        	g = output.createGraphics();
	        	g.setColor(Color.white);
	        	g.fillRect(output.getWidth()-4, output.getHeight()-4, 4, 4);
        	}
        	writeImage(output, outputDir + File.separatorChar + "trial", trial + "-" + simStepCount);
        }
    	
    	
		if ((targetPerformanceType == 1 && p >= targetPerformance) || (targetPerformanceType == 0 && p <= targetPerformance)) {
			//perform tests
			int percentCorrect = 0;
	    	for (int test = 0; test < numTests; test++) {
				sim.resetAgents();
	        	sim.initBehaviors();
	        	//set robot and target(s) to initial positions for this trial
	        	robot[threadIndex].resetPositionRotationAt(testPositionsRobot[test], testRotationsRobot[test]);
	        	for (int obj = 0; obj < numObjects; obj++) {
	    		    objects[threadIndex][obj].resetPositionRotationAt(testPositionsObjects[test][obj], testRotationsObjects[test][obj]);
	        	}
	        	int simStepCount;
	        	previousRobotCentre = null;
	        	int stalledCount = 0;
	        	int maxSimStepsTest = Math.round(maxTestTime * frameRate);
	        	boolean collided = false;
	        	for (simStepCount = 0; simStepCount < maxSimStepsTest; simStepCount++) {
	            	output = generateImagesForSimStep(genotype, substrate, threadIndex, true); //calls sim.simulateOneStep
	        		
	            	robot[threadIndex].getCoords(robotCentre);
	            	objects[threadIndex][0].getCoords(targetCentre);
	            	
	            	//double dist = robotCentre.distance(targetCentre) - (robotSize + objectSize) / 2;
	            	//if (dist <= robotSpeed / frameRate) { //if within the distance the robot can move in one time step
	            	if (objects[threadIndex][0].interactionDetected()) {
	            		percentCorrect++;
	                	collided = true;
	                }
	                
	                writeImage(output, outputDir + File.separatorChar + "test", test + "-" + simStepCount);
	            	
	            	if (collided) {
	            		simStepCount++;
	            		break;
	            	}
	            	
	            	robot[threadIndex].getCoords(robotCentre);
	            	if (previousRobotCentre != null && robotCentre.distance(previousRobotCentre) < robotSpeed*0.01 && 
	                		Math.abs(robot[threadIndex].getRotationalVelocity()) < Math.toRadians(5))
	                	stalledCount++;
	                else
	                	stalledCount = 0;
	                
	                //if we've stalled (eg become stuck on something) then end simulation
	                if (stalledCount > stalledTimeOut * frameRate) {
	            		//System.out.println("stalled");
	                	break;
	            	}
	                previousRobotCentre = (Point3d) robotCentre.clone();
	            }
	            output = generateImagesForSimStep(genotype, substrate, threadIndex, false);
	            if (collided) {
		            g = output.createGraphics();
		        	g.setColor(Color.white);
		        	g.fillRect(output.getWidth()-4, output.getHeight()-4, 4, 4);
	            }
	            writeImage(output, outputDir + File.separatorChar + "test", test + "-" + simStepCount);
			}
	    	
	    	logger.info("Test performance for " + genotype.getId() + ": " + ((float) percentCorrect / numTests));
		}
    }
    
    
    private BufferedImage generateImagesForSimStep(Chromosome genotype, com.anji.hyperneat.GridNet substrate, int threadIndex, boolean simOneStep) {
    	Simulator sim = simulator[threadIndex];
    	
    	BufferedImage worldView = sim.getWorld().renderToImage();
		
    	if (simOneStep) //false if we're just getting final view of the world
    		sim.simulateOneStep();
    	
		//BufferedImage robotView = robot[threadIndex].getView();
		
		//generate image for activation values for network for this step
		float[][][] activation = substrate.getActivation();
		BufferedImage[] activationImage = new BufferedImage[depth];
		int maxImageLayerWidth = 0;
		int imageLayerTotalHeight = 0;
		for (int layer = 0; layer < depth; layer++) {
			maxImageLayerWidth = Math.max(maxImageLayerWidth, width[layer]*imageScaleActivation[layer]);
			imageLayerTotalHeight += height[layer]*imageScaleActivation[layer] + imageSpacing;
			
			activationImage[layer] = new BufferedImage(width[layer]*imageScaleActivation[layer], height[layer]*imageScaleActivation[layer], BufferedImage.TYPE_BYTE_GRAY);
			Graphics2D canvas = activationImage[layer].createGraphics();
			
			for (int y = 0; y < height[layer]; y++) {
                for (int x = 0; x < width[layer]; x++) {
                	int color = (int) (activation[layer][y][x] * 255); //assumes output range is [0, 1]
					canvas.setColor(new Color(color, color, color));
					canvas.fillRect(x * imageScaleActivation[layer], y * imageScaleActivation[layer], imageScaleActivation[layer], imageScaleActivation[layer]);
                }
			}
		}
		imageLayerTotalHeight -= imageSpacing;
		
		int imageWidth = worldView.getWidth() + maxImageLayerWidth + imageSpacing;
		int imageHeight = Math.max(worldView.getHeight(), imageLayerTotalHeight);
        BufferedImage output = new BufferedImage(imageWidth, imageHeight, BufferedImage.TYPE_INT_RGB);
		Graphics2D g = output.createGraphics();
		g.setColor(new Color(0, 0, 127));
		g.fillRect(0, 0, imageWidth, imageHeight);
		
		int xOffset = 0;
		int yOffset = 0;
		g.drawImage(worldView, xOffset, yOffset, null);
		xOffset += worldView.getWidth() + imageSpacing;
		
		for (int layer = 0; layer < depth; layer++) {
			g.drawImage(activationImage[layer], xOffset, yOffset, null);
			yOffset += activationImage[layer].getHeight() + imageSpacing;
		}
		
		return output;
    }
    
    
    private void writeImage(BufferedImage image, String dir, String name) {
    	File dirFile = new File(dir);
		if (!dirFile.exists() && !dirFile.mkdirs()) {
        	logger.error("Error creating directory: " + dir);
		}
		else {
			String fullPath = dir + File.separatorChar + name + ".png";
	    	try {
	    		ImageIO.write(image, "png", new File(fullPath));
	        }
	        catch (IOException e) {
	        	logger.error("Error writing image: " + fullPath);
	        }
		}
    }
    
    
    class Robot extends Agent {
    	float speed;
    	int cameraWidth, cameraHeight;
        CameraSensor camera;
        GridNet network;
        BufferedImage view;
        int id;
        
        public Robot(Vector3d position, int id, float speed, float width, float height, GridNet network) {
            super(position, "Robot "+id);
            this.id = id;
            this.speed = speed;
            radius = width/2;
            float cameraBodyRadius = radius/2;
            this.height = height - cameraBodyRadius;
                        // body
            Color3f color = new Color3f(0,1,0);
	        Appearance appear = new Appearance();
	        material.setDiffuseColor(color);
	        material.setAmbientColor(color);
	        material.setSpecularColor(new Color3f(0,0,0));
	        appear.setMaterial(material);
	        int flags = Primitive.GEOMETRY_NOT_SHARED | Primitive.ENABLE_GEOMETRY_PICKING | Primitive.GENERATE_NORMALS | Primitive.ENABLE_APPEARANCE_MODIFY;
	        body = new Cylinder(radius,height,flags,appear);
	        //body = new Sphere(radius,flags,appear);
	        //body = new com.sun.j3d.utils.geometry.Box(size,size,size,flags,appear);
            
	        setKinematicModel(new DifferentialKinematic(width));
	        
            this.network = network;
            this.cameraHeight = network.getInputDimension()[0];
            this.cameraWidth = network.getInputDimension()[1];
            
            //float aspectRatio = (float)cameraWidth / cameraHeight;
            float aspectRatio = (float)cameraHeight / cameraWidth;
            
            //dimensions of physical view screen
            float viewWidth = 1f;
            float viewHeight = viewWidth * aspectRatio;
            
            Vector3d camPos = new Vector3d(0, (height / 2) + cameraBodyRadius*0.95, 0);
            camera = RobotFactory.addCameraSensor(this, camPos, cameraBodyRadius, cameraWidth, cameraHeight, viewWidth, viewHeight, robotFOV, antialias);
            camera.setUpdateOnEachFrame(true);
        }
        
        public void setNetwork(GridNet net) {
        	network = net;
        }
        
        /** Initialize Agent's Behavior */
        public void initBehavior() {
            
        }
        
        /** Perform one step of Agent's Behavior */
        public void performBehavior() {
        	TimeProfiler.markStart("network", id);
        	//input current camera image to network
        	float[][] stimuli = network.getInputs();
        	camera.copyVisionImage(stimuli);
        	float[][] responses = network.next(stimuli);
        	
        	float leftWheelSpeed = (responses[0][0] * 1.5f - 0.5f) * robotSpeed;
        	float rightWheelSpeed = (responses[0][1] * 1.5f - 0.5f) * robotSpeed;
            ((DifferentialKinematic) kinematicModel).setWheelsVelocity(leftWheelSpeed, rightWheelSpeed);
            //((DifferentialKinematic) kinematicModel).setWheelsVelocity(1, 1);
        	//((DifferentialKinematic) kinematicModel).setWheelsVelocity(robotSpeed, robotSpeed);
            
            TimeProfiler.markFinish("network", id);
        }
        
        public void setWheelsVelocity(float left, float right) {
            ((DifferentialKinematic) kinematicModel).setWheelsVelocity(left, right);
        }
        
        public BufferedImage getView() {
        	if (view == null)
        		view = camera.createCompatibleImage();
        	camera.copyVisionImage(view);
        	return view;
        }
    }
    
    class RNObject extends Agent {
    	float speed;
    	
        public RNObject(Vector3d position, String name, float speed, float width, float height, Color3f colour, String shape) {
            super(position, name);
            this.speed = speed;
            this.height = height;
            radius = width/2;
            
            // body
            Appearance appear = new Appearance();
	        material.setDiffuseColor(colour);
	        material.setAmbientColor(colour);
	        material.setSpecularColor(new Color3f(0,0,0));
	        appear.setMaterial(material);
	        int flags = Primitive.GEOMETRY_NOT_SHARED | Primitive.ENABLE_GEOMETRY_PICKING | Primitive.GENERATE_NORMALS | Primitive.ENABLE_APPEARANCE_MODIFY;
	        
	        if (shape.equals("cylinder"))
	        	body = new Cylinder(radius,height,flags,appear);
	        else if (shape.equals("sphere"))
		        body = new Sphere(radius,flags,appear);
	        else
	    	    body = new com.sun.j3d.utils.geometry.Box(width,width,width,flags,appear);
        }
        
        /** Initialize Agent's Behavior */
        public void initBehavior() {
            
        }
        
        /** Perform one step of Agent's Behavior */
        public void performBehavior() {
        }
    }
    
    class Environment extends EnvironmentDescription {
    	public Environment(float width, float height, Agent[] agents) {
    		super();
    		backgroundColor = darkgray;
    		floorColor = darkgray;
    		wallColor = gray;
    		hasAxis = false;
    		light1Position = null;
    		light2Position = null;
    		worldWidth = width;
    		worldHeight = height;
    		worldViewPoint = World.VIEW_FROM_TOP;
    		usePhysics = true;
    		enableMouseControl = false;
    		for (Agent a : agents) {
    		  add(a);	
    		}
    	}
    }
    
    public void dispose() {
    	for (int threadIndex = 0; threadIndex < getNumThreads(); threadIndex++) {
    		simulator[threadIndex].dispose();
    		world[threadIndex].dispose();
    	}
    }
}

